import copy
import math
import struct


class ValueStruct:
    def __init__(self, seq, device_id, value):
        self.seq = seq
        self.device_id= device_id
        self.data= copy.deepcopy(value)

class BasicCmd:
    def __init__(self):
        self.cmd_id= 0
        self.cmd_len = 0
        self.value:ValueStruct = None

    def set_cmd_id(self, id):
        self.cmd_id = id

    def set_cmd_len(self, l1):
        self.cmd_len = l1

    def set_cmd_value(self, bytes1):
        self.value = ValueStruct(bytes1[0], bytes1[1], bytes1[2:])

    def get_device_id(self):
        return self.value.device_id

    def generate_send_bytes(self):
        pass

    @classmethod
    def which_cmd(cls, cmd_id):
        for subclass in cls.__subclasses__():
            if subclass.CMD_ID == cmd_id:
                return subclass()
        raise ValueError(f"没有找到 CMD_ID 为 {cmd_id} 的子类")

class HeadtrackLock(BasicCmd):
    '''
    这个命令只为接受数据，并展示到3维度的界面
    '''

    CMD_ID = 0x04
    def __init__(self):
        super().__init__()

    def generate_send_bytes(self):
        result = "HeadtrackLock:device_id={}, locked:{}"
        deviceid = self.value.device_id
        locked = int(self.value.data[0])


        return result.format(deviceid, locked )



class HeadtrackEulerAngle(BasicCmd):
    '''
    这个命令只为接受数据，并展示到3维度的界面
    '''

    CMD_ID = 0x05
    def __init__(self):
        super().__init__()

    def generate_send_bytes(self):
        result = "HeadtrackEulerAngle:device_id={}, roll:{:.2f}, pitch:{:.2f},yaw:{:.2f}"
        deviceid = self.value.device_id

        roll = struct.unpack('f', self.value.data[0:4])[0]
        pitch = struct.unpack('f', self.value.data[4:8])[0]
        yaw = struct.unpack('f', self.value.data[8:12])[0]

        return result.format(deviceid, roll, pitch, yaw)

    def get_roll(self):
        return struct.unpack('f', self.value.data[0:4])[0];

    def get_pitch(self):
        return struct.unpack('f', self.value.data[4:8])[0]

    def get_yaw(self):
        return struct.unpack('f', self.value.data[8:12])[0]



class GimbalStatus(BasicCmd):
    CMD_ID = 0x01
    def __init__(self):

        super().__init__()
        self.velocity_pid_error = 0
        self.angle_pid_error = 0


    def generate_send_bytes(self):
        result = "GimbalStatus:device_id={}, motor_id:{},速度pid error:{:.2f}, 角度pid error:{:.2f},电角度(弧):{:.2f},机械角度(弧):{:.2f},零电角度(弧):{:.2f}"
        deviceid = self.value.device_id
        motor_id = int(self.value.data[0])

        self.velocity_pid_error = struct.unpack('f', self.value.data[1:5])[0]
        self.angle_pid_error = struct.unpack('f', self.value.data[5:9])[0]
        # 原始数据是弧度
        e_angle = struct.unpack('f', self.value.data[9:13])[0]
        m_angle = struct.unpack('f', self.value.data[13:17])[0]
        zero_e_angle = struct.unpack('f', self.value.data[17:21])[0];

        return result.format(deviceid,motor_id, self.velocity_pid_error, self.angle_pid_error, e_angle, m_angle,zero_e_angle  )

    def get_velocity_pid_error(self):
        return self.velocity_pid_error

    def get_angle_volocity_pid_error(self):
        return self.angle_pid_error



